%% 优化路径
% 消除结尾处无效往返路径
% 消除“之”字形无效折返路径（暂未实现）
function [newX,newY,newTheta,newD,newDelta] = OptimizePath(x,y,theta,D,delta,endX,endY,Configure)
    % 消除目标无效往返路径
    % 从终点开始，找到第一个在终点范围内的点
    EndTolerance = Configure.END_TOLERANCE;
    endIndex = length(x);
    for i = length(x):-1:1
        if abs(x(i)-endX)<EndTolerance && abs(y(i)-endY)<EndTolerance
            endIndex = i;
        end
    end
    if endIndex ~= length(x)
        newX = x(1:endIndex);
        newY = y(1:endIndex);
        newTheta = theta(1:endIndex);
        newD = D(1:endIndex);
        newDelta = delta(1:endIndex);
    else
        newX = x;
        newY = y;
        newTheta = theta;
        newD = D;
        newDelta = delta;
    end

    % 最后一个点为终点
    newX(end+1) = endX;
    newY(end+1) = endY;
    newTheta(end+1) = atan2(newY(end)-newY(end-1),newX(end)-newX(end-1));
    newD(end+1) = newD(end);
    newDelta(end+1) = newDelta(end);
end